#include "PID.h"

void PIDController_Init(PIDController *pid)
{

    /* Clear controller variables */
    pid->integrator = 0.0f;
    pid->prevError = 0.0f;

    pid->differentiator = 0.0f;
    pid->prevMeasurement = 0.0f;

    pid->out = 0.0f;
}

float PIDController_Update(PIDController *pid, float setpoint, float measurement)
{

    /*
     * Error signal
     */
    float error = setpoint - measurement;

    /*
     * Proportional
     */
    float proportional = pid->kp * error;

    /*
     * Integral
     */
    pid->integrator = pid->integrator + 0.5f * pid->ki * pid->T * (error + pid->prevError);

    /* Anti-wind-up via integrator clamping */
    if (pid->integrator > pid->limMaxInt)
    {

        pid->integrator = pid->limMaxInt;
    }
    else if (pid->integrator < pid->limMinInt)
    {

        pid->integrator = pid->limMinInt;
    }

    /*
     * 导数（带限微分器）
     */
    /* Note: derivative on measurement, therefore minus sign in front of equation! */
    pid->differentiator = -(2.0f * pid->kd * (measurement - pid->prevMeasurement) + (2.0f * pid->tau - pid->T) * pid->differentiator) /
                          (2.0f * pid->tau + pid->T);

    /*
     * 计算输出并应用限制
     */
    pid->out = proportional + pid->integrator + pid->differentiator;

    if (pid->out > pid->limMax)
    {

        pid->out = pid->limMax;
    }
    else if (pid->out < pid->limMin)
    {

        pid->out = pid->limMin;
    }

    /* 存储错误和测量以便以后使用 */
    pid->prevError = error;
    pid->prevMeasurement = measurement;

    /* 返回控制器输出 */
    return pid->out;
}
